//#define RUN_VIDEO
#ifdef RUN_VIDEO

#include"ARMOR/armor_detect.h"
#include"ARMOR/rp_predict.h"

#include"OTHERS/share_memory.h"
#include<fstream>
#include"daheng/save_video.h"
#include"my_predict/myPredict.h"

//#define WRITE_VIDEO

void gamma_transform(Mat src, double gamma)
{
    float gfacor=1/gamma;
    uchar lutData[256];
    for(int i=0;i<256;i++)
    {
        float f=(i+0.5f)/255;
        f=(float)(pow(f,gfacor));
        lutData[i]=saturate_cast<uchar>(f*255.0f-0.5f);
    }

    Mat lut(1,256,CV_8UC1,lutData);
    Mat result;
    LUT(src,lut,result);
    namedWindow("result",0);
    imshow("result",result);
}

int main()
{
    ofstream yaw("yaw.txt");

    // 相机内参
    Mat cam = (Mat_<double>(3, 3) << 1698, 3.2, 672,
                                     0.0000000000000000, 1697, 545.8,
                                     0.0000000000000000, 0.0000000000000000, 1.0000000000000000);
    // 畸变系数
    Mat dis = (Mat_<double>(5, 1) << -0.1156, 0.4920, -0.7336,
                                    -0.007, 0.0022);

    Mat frame;
    int video_index = 0;
    VideoCapture capture;
    capture.open("/home/nuc/Desktop/build-shaobing_test-Desktop_Qt_5_9_1_GCC_64bit-Debug/Tue May 24 16:13:20 2022.avi");
    if(!capture.isOpened())
    {
        cout<<"open video failed"<<endl;
        return -1;
    }
    long totalFrameNumber=capture.get(CV_CAP_PROP_FRAME_COUNT);//获取视频的总帧数
    cout << "整个视频共: " << totalFrameNumber << "帧" << endl;
    long frame_index =0;
    capture.set(CV_CAP_PROP_POS_FRAMES, frame_index);

#ifdef WRITE_VIDEO
    SaveVideo video_writer;
#endif
    int iscontinue=0;
    int mode=1;
    int my_color=2;
    ArmorDetector armordetector;
    my_predict my_pre;

    CShareMemory draw_line1("txh", 128);
    CShareMemory draw_line2("txh2", 128);
    namedWindow("src",0);
    namedWindow("ori_img",0);
    while(1)
    {
        video_index++;
        cout<<"*************当前帧数  "<<video_index<<"*******************"<<endl;
        capture >> frame;
        if(frame.empty())
        {
            cout<<"video finish"<<endl;
            return 0;
        }
        if(video_index<200) continue;

#ifdef WRITE_VIDEO
        if(video_index>43400) break;
        video_writer.updateImage(frame);
#endif

#ifndef WRITE_VIDEO
//        cvtColor(frame,frame,cv::COLOR_RGB2BGR);
        int height=frame.rows;
        int width=frame.cols;
//        Mat pad_img=Mat::zeros(Size(VIDEO_WIDTH,VIDEO_HEIGHT), CV_8UC3);
//        Rect rrrr(VIDEO_WIDTH-width, VIDEO_HEIGHT-height, width, height);
//        frame.copyTo(pad_img(rrrr));
        imshow("ori_img",frame);


        Data rdata;
        armordetector.src = frame.clone();
        armordetector.set_img(1);
        armordetector.pretreat_img(my_color,mode);
        armordetector.find_light();
        armordetector.match_lights(mode);
        armordetector.match_lights_deeper();
        armordetector.choose_target(my_color);
        armordetector.get_armor_info(cam, dis);


/////数字识别
//        Mat num_img;

//        num_img=armordetector.get_num_img(frame);

//        namedWindow("num_img", WINDOW_NORMAL);
//        imshow("num_img", num_img);

//        gamma_transform(num_img,2);



        armordetector.info_src(rdata);

        int drop_flag = 0;
        if(armordetector.drop_frame_cnt <= 20 && armordetector.drop_frame_cnt > 0)
        {
            armordetector.target_armor.empty = 0;
            drop_flag = 1;
        }
        if(armordetector.target_armor.empty)
        {
            armordetector.target_armor.armor_reset();
            armordetector.pre_target.armor_reset();
            my_pre.reset_predictor();
            cout<<"reset armor "<<endl;
        }
        else
        {
            if(!drop_flag)
            {
                my_pre.init_coordinate();
                my_pre.update_abs_info(armordetector.target_armor.pitch,armordetector.target_armor.yaw);
            }
            my_pre.predict_next_pos(armordetector.drop_frame_cnt);
        }
        draw_line1.Data2Memory(my_pre.yaw_out);  //blue
        draw_line2.Data2Memory(armordetector.target_armor.yaw);  //orange

        imshow("src",armordetector.src);

        int comm= (int)waitKey(10);
        if(comm=='s')
        {
            iscontinue=0;
        }

        if(iscontinue==1)
        {
            waitKey(1);
            int delay = 10;
            if(delay >= 0 && waitKey(delay) >= 32) waitKey(1);
        }
        else if(iscontinue==0)
        {
            int comm2=waitKey(0);
            if(comm2=='c')
                iscontinue=1;
            int delay = 30;
            if(delay >= 0 && waitKey(delay) >= 32) waitKey(0);
        }

#endif
    }
    yaw.close();

}

#endif
